﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using System.Collections;

namespace Demo_SteeringBehaviour
{


    public class SteeringBehaviour
    {

        private struct Agent_Rank
        {
            public Agent agent;
            public float distance;
        };
        
        
        
        #region DATA MEMBERS
        //
        private List<Agent> agents;
        private behaviour_Type selectedBehaviour;

        private static Random rand = new Random();
        //
        #endregion DATA MEMBERS


        #region OBJECT_CONSTRUCTION
        //
        public SteeringBehaviour()
        {
            this.selectedBehaviour = behaviour_Type.NEUTRAL;
            Init();
        }

        //
        public SteeringBehaviour(List<Agent> agents)
        {
            this.selectedBehaviour = behaviour_Type.NEUTRAL;
            Init();
            this.agents = agents;
        }

        //Lazy Instantiation of List<Agent>
        private void Init()
        {
            if ( agents == null)
            {
                this.agents = new List<Agent>();
            }
            else { agents.Clear(); }
        }
        //
        #endregion


        #region PROPERTIES
        //
        public List<Agent> AgentList
        {
            get { return this.agents; }
        }
        //
        public behaviour_Type SelectedBehaviour
        {
            get { return this.selectedBehaviour; }
        }
        //
        #endregion

        //QuickSort Method
        private List<Agent> Sort(List<Agent_Rank> target)
        {
            int prt = 0, counter = 0;
            Agent_Rank temp = target[0];

            //Sort list: Max->Min (E.g.: 10, 9, 8...1)
            while (prt < target.Count)
            {
                for (int i = 0; i < target.Count; ++i)
                {
                    if (target[prt].distance > target[i].distance)
                    {
                        temp = target[prt];
                        target[prt] = target[i];
                        target[i] = temp;
                    }
                    ++prt;
                }
            }

            //transfer Custom list to AgentList
            List<Agent> sortedList = new List<Agent>();
            for (int i = 0; i < target.Count; ++i)
            {
                sortedList.Add(target[i].agent);
            }

            return sortedList;
        }

        //Public Method, call to decide Behaviour...
        public behaviour_Type SelectBehaviour(Agent self, List<Agent> agents)
        {
            #region Local Vars
            //List<Agent> panicList = null, cautionList = null, seekList = null;
            List<Agent_Rank> panicList1 = new List<Agent_Rank>();
            List<Agent_Rank> cautionList1 = new List<Agent_Rank>();
            List<Agent_Rank> seekList1 = new List<Agent_Rank>();
            
            Vector2 diff = new Vector2();
            float distance = 0.0f;
            bool isCaptured = false;
            #endregion

            for (int i = 0; i < agents.Count; ++i ) //foreach (Agent target in agents)
            {
                //Same Species Check = FALSE
                if (self.AgentType != agents[i].AgentType)
                {
                    diff = agents[i].Position - self.Position;
                    distance = (float)Math.Round(Math.Sqrt((diff.X * diff.X) + (diff.Y * diff.Y)), 2);

                    //temp Agent_Rank
                    Agent_Rank temp = new Agent_Rank();

                    switch ((int)agents[i].AgentType)
                    {
                        #region CASE: TARGET = ROCK
                        case (int)agent_Type.ROCK:
                            if (self.AgentType == agent_Type.SCISSOR)
                            {
                                //Agent 'ME' has been captured
                                if (distance <= self.Flee_Capture_Distance)
                                {
                                    isCaptured = true;
                                    break;
                                }
                                //Agent 'ME' Panic, try to Escape
                                else if (distance <= self.Flee_Panic_Distance)
                                {
                                    ////if (panicList1 == null)//(panicList == null) 
                                    ////{
                                    ////    //panicList = new List<Agent>();
                                    ////    panicList1 = new List<Agent_Rank>();
                                    ////}
                                    temp.agent = agents[i];
                                    temp.distance = distance;
                                    //panicList.Add(target);
                                    panicList1.Add(temp);
                                }
                                //Agent 'ME' Flee Cautiously
                                else if (distance <= self.Flee_Caution_Distance)
                                {
                                    ////if (cautionList1 == null) //(cautionList == null) 
                                    ////{
                                    ////    //cautionList = new List<Agent>();
                                    ////    cautionList1 = new List<Agent_Rank>();
                                    ////}
                                    temp.agent = agents[i];
                                    temp.distance = distance;
                                    //cautionList.Add(target);
                                    cautionList1.Add(temp);
                                }
                            }
                            else if (self.AgentType == agent_Type.PAPER && distance <= agents[i].SeekDistance)
                            {
                                ////if (seekList1 == null) //(seekList == null) 
                                ////{ 
                                ////    //seekList = new List<Agent>(); 
                                ////    seekList1 = new List<Agent_Rank>();
                                ////}
                                temp.agent = agents[i];
                                temp.distance = distance;
                                //seekList.Add(target);
                                seekList1.Add(temp);
                            }
                            break;
                        #endregion

                        #region CASE: TARGET = PAPER
                        case (int)agent_Type.PAPER:
                            if (self.AgentType == agent_Type.ROCK)
                            {
                                //Agent 'ME' has been captured
                                if (distance <= self.Flee_Capture_Distance)
                                {
                                    isCaptured = true;
                                    break;
                                }
                                //Agent 'ME' Panic, try to Escape
                                else if (distance <= self.Flee_Panic_Distance)
                                {
                                    ////if (panicList1 == null)//(panicList == null) 
                                    ////{
                                    ////    //panicList = new List<Agent>();
                                    ////    panicList1 = new List<Agent_Rank>();
                                    ////}
                                    temp.agent = agents[i];
                                    temp.distance = distance;
                                    //panicList.Add(target);
                                    panicList1.Add(temp);
                                }
                                //Agent 'ME' Flee Cautiously
                                else if (distance <= self.Flee_Caution_Distance)
                                {
                                    ////if (cautionList1 == null) //(cautionList == null) 
                                    ////{
                                    ////    //cautionList = new List<Agent>();
                                    ////    cautionList1 = new List<Agent_Rank>();
                                    ////}
                                    temp.agent = agents[i];
                                    temp.distance = distance;
                                    //cautionList.Add(target);
                                    cautionList1.Add(temp);
                                }
                            }
                            else if (self.AgentType == agent_Type.SCISSOR && distance <= agents[i].SeekDistance)
                            {
                                ////if (seekList1 == null) //(seekList == null) 
                                ////{
                                ////    //seekList = new List<Agent>(); 
                                ////    seekList1 = new List<Agent_Rank>();
                                ////}
                                temp.agent = agents[i];
                                temp.distance = distance;
                                //seekList.Add(target);
                                seekList1.Add(temp);
                            }
                            break;
                        #endregion

                        #region CASE: TARGET = SCISSOR
                        case (int)agent_Type.SCISSOR:
                            if (self.AgentType == agent_Type.PAPER)
                            {
                                //Agent 'ME' has been captured
                                if (distance <= self.Flee_Capture_Distance)
                                {
                                    isCaptured = true;
                                    break;
                                }
                                //Agent 'ME' Panic, try to Escape
                                else if (distance <= self.Flee_Panic_Distance)
                                {
                                    ////if (panicList1 == null)//(panicList == null) 
                                    ////{
                                    ////    //panicList = new List<Agent>();
                                    ////    panicList1 = new List<Agent_Rank>();
                                    ////}
                                    temp.agent = agents[i];
                                    temp.distance = distance;
                                    //panicList.Add(target);
                                    panicList1.Add(temp);
                                }
                                //Agent 'ME' Flee Cautiously
                                else if (distance <= self.Flee_Caution_Distance)
                                {
                                    ////if (cautionList1 == null) //(cautionList == null) 
                                    ////{
                                    ////    //cautionList = new List<Agent>();
                                    ////    cautionList1 = new List<Agent_Rank>();
                                    ////}
                                    temp.agent = agents[i];
                                    temp.distance = distance;
                                    //cautionList.Add(target);
                                    cautionList1.Add(temp);
                                }
                            }
                            else if (self.AgentType == agent_Type.ROCK && distance <= agents[i].SeekDistance)
                            {
                                ////if (seekList1 == null) //(seekList == null) 
                                ////{
                                ////    //seekList = new List<Agent>(); 
                                ////    seekList1 = new List<Agent_Rank>();
                                ////}
                                temp.agent = agents[i];
                                temp.distance = distance;
                                //seekList.Add(target);
                                seekList1.Add(temp);
                            }
                            break;
                        #endregion
                    }//switch
                }//Same Species Check = FALSE
            }//foreach

            #region DECIDE_BEHAVIOUR
            //Agent 'ME' = Captured
            if (isCaptured)
            {
                //isCaptured = false;
                this.agents.Clear();
                this.selectedBehaviour = behaviour_Type.CAUGHT;
                
            }
            //Agent 'ME' = Flee_Panic
            else if (panicList1.Count != 0) //(panicList != null || panicList.Count != 0)
            {
                this.agents = Sort(panicList1);
                this.selectedBehaviour = behaviour_Type.FLEE_PANIC;
            }
            //Agent 'ME' = Flee_Caution
            else if (cautionList1.Count != 0) //(cautionList != null || cautionList.Count != 0)
            {
                this.agents = Sort(cautionList1);
                this.selectedBehaviour = behaviour_Type.FLEE_CAUTION;
            }
            //Agent 'ME' = Seek
            else if (seekList1.Count != 0) //(seekList != null || seekList.Count != 0)
            {
                this.agents = Sort(seekList1);
                this.selectedBehaviour = behaviour_Type.SEEK;
            }
            //Agent 'ME' = Wander
            else
            {
                this.agents.Clear();
                this.selectedBehaviour = behaviour_Type.WANDER;
            }
            #endregion

            return this.selectedBehaviour;
        }


        #region STEERING BEHAVIOURS
        //
        //-------------------------------------------------------------------------
        //FLEE_PANIC: 
        //TODO: Implement this Method
        public float Flee_Panic(Agent self, List<Agent> target)
        {
            float thetaInRadians_Max = 0.0f, thetaInRadians_Avg = 0.0f;
            int counter = -1;
            Vector2 desired = new Vector2();

            for (int i = 0; i < target.Count; ++i)
            {
                desired = new Vector2((self.Position.X - target[i].Position.X), (self.Position.Y - target[i].Position.Y));

                //Normalize only if Vector != Zero
                if (desired != Vector2.Zero) { desired.Normalize(); }

                thetaInRadians_Avg = (float)Math.Atan2(desired.Y, desired.X);
                thetaInRadians_Avg = MathHelper.WrapAngle(thetaInRadians_Avg);

                //Only one Item in list
                if (i == 0)
                {
                    thetaInRadians_Max = thetaInRadians_Avg;
                    thetaInRadians_Avg = 0.0f;
                }
                else
                {
                    thetaInRadians_Avg += thetaInRadians_Avg;
                }
                counter = i;
            }

            //Take Avg theta of all target agents in range
            thetaInRadians_Avg /= counter;
            //Add the flee theta of closest agent w/ avg thteta of other agents in range
            thetaInRadians_Max += thetaInRadians_Avg;

            return (MathHelper.WrapAngle((float)Math.Round(thetaInRadians_Max, 2)));
        }


        //-------------------------------------------------------------------------
        //FLEE_CAUTION: 
        //TODO: Implement this Method
        public float Flee_Caution(Agent self, List<Agent> target)
        {
            float thetaInRadians_Max = 0.0f, thetaInRadians_Avg = 0.0f;
            int counter = -1;
            Vector2 desired = new Vector2();

            for (int i = 0; i < target.Count; ++i)
            {
                desired = new Vector2((self.Position.X - target[i].Position.X), (self.Position.Y - target[i].Position.Y));

                //Normalize only if Vector != Zero
                if (desired != Vector2.Zero) { desired.Normalize(); }

                thetaInRadians_Avg = (float)Math.Atan2(desired.Y, desired.X);
                thetaInRadians_Avg = MathHelper.WrapAngle(thetaInRadians_Avg);

                //Only one Item in list
                if (i == 0)
                {
                    thetaInRadians_Max = thetaInRadians_Avg;
                    thetaInRadians_Avg = 0.0f;
                }
                else
                {
                    thetaInRadians_Avg += thetaInRadians_Avg;
                }
                counter = i;
            }
            
            //Take Avg theta of all target agents in range
            thetaInRadians_Avg /= counter;
            //Add the flee theta of closest agent w/ avg thteta of other agents in range
            thetaInRadians_Max += thetaInRadians_Avg;
            
            return (MathHelper.WrapAngle((float)Math.Round(thetaInRadians_Max, 2)));
        }


        //-------------------------------------------------------------------------
        //SEEK: Returns NEW thetaInRadians - based on first agent in Index-Zero, (a.k.a: change in direction)
        //TargetList is sorted Index-Zero = Max
        public float Seek(Agent self, List<Agent> target)
        {
            float thetaInRadians = 0.0f;
            Vector2 desired = new Vector2((target[0].Position.X - self.Position.X), (target[0].Position.Y - self.Position.Y));

            //Normalize only if Vector != Zero
            if (desired != Vector2.Zero) { desired.Normalize(); }

            //thetaInRadians = (float)Math.Acos(Vector2.Dot(seeker.Direction, target.Direction));
            thetaInRadians = (float)Math.Atan2(desired.Y, desired.X);
            thetaInRadians = MathHelper.WrapAngle(thetaInRadians);

            return ((float)Math.Round(thetaInRadians, 2));
        }


        //-------------------------------------------------------------------------
        //WANDER Behaviour - decently working but still has a little jitter
        //TODO: Implement this Method
        public float Wander(Agent self)
        {
            float thetaInRadians = 0.0f;
            Vector2 wanderTarget_Position = new Vector2();
            //Generate a Random vector btw (-1 - 1),  Mu;tiply w/ rotation speed
            Vector2 wanderTarget_Direction = new Vector2(MathHelper.Lerp(-1f, 1f, (float)rand.NextDouble()));


            //Normalize this new Random jittered vector
            if (wanderTarget_Direction != Vector2.Zero)
            {
                wanderTarget_Direction.Normalize();
            }

            //Transform the 'targetDirection' Vector to the wandering circle's parameter
            //This is in local coordinates
            wanderTarget_Direction.X *= self.WanderRadius;
            wanderTarget_Direction.Y *= self.WanderRadius;

            //Project 'wanderDistance' to self.Center
            Vector2 displacement = new Vector2(self.Direction.X * self.WanderDistance, self.Direction.Y * self.WanderDistance);

            //Transform the 'wanderTarget' position from local to world coordinates
            //WanderDistance is the displacement, how far in front of the agent the wandering circle is placed
            wanderTarget_Position = self.Center + wanderTarget_Direction + displacement;

            Vector2 displacement_Desired = wanderTarget_Position - self.Center;
            displacement_Desired.Normalize();

            //Calc the target's thetaRadians OR 'Desired Theta'
            float desiredAngle = (float)Math.Atan2(displacement_Desired.Y, displacement_Desired.X);
            //Wrap the theta within 'arcTan' funct boundary
            float difference = MathHelper.WrapAngle(desiredAngle - self.AngleInRadians);

            //Take Change theta and Clamp it within it's rotation speed boundary
            difference = MathHelper.Clamp(difference, -self.Rotation_Normal, self.Rotation_Normal);
            //Wrap the theta within boundary
            thetaInRadians = MathHelper.WrapAngle(difference);

            //return the theta(radians) that will be used to find the new Direction the agent should wander
            return ((float)Math.Round(thetaInRadians, 2));
        }
        //
        #endregion
    }//class
}//namespace